#include <math.h>
#include <stdlib.h>
#include "HexapodGaitGenerator.h"

const float G_PI = 3.1415926;

void gaitGeneratorInit(GaitGenerator* gaitG) {
	gaitG->firstRun = true;
	
}

void gaitSetGait(GaitGenerator* gaitG, float vxyGoalGlobal[2], float dYawGoal, float gaitHeight){
	gaitG->vxyGoal[0] = vxyGoalGlobal[0];
	gaitG->vxyGoal[1] = vxyGoalGlobal[1];
	gaitG->dYawGoal = dYawGoal;
	gaitG->gaitHeight = gaitHeight;
}

void gaitRestart(GaitGenerator* gaitG){
  gaitG->firstRun = true;
  gaitG->vxyGoal[0] = 0;
  gaitG->vxyGoal[1] = 0;
}


void gaitRun(GaitGenerator* gaitG, Point3D *feetPos){
	if(gaitG->firstRun){
    // for(uint8_t i; i < LEG_CNT; i++) {
    //   gaitG->startP[i] = 
    // }
    // gaitG->startP = est->getFeetPos(); // 闭环
    gaitG->firstRun = false;
	}

	for(uint8_t i; i < LEG_CNT; i++) {
    if(gaitG->contact[i] == 1) {
      if(gaitG->phase[i] < 0.5) {
        // 闭环
        // gaitG->startP[i] = _est->getFootPos[i];
      }
      (feetPos + i)->x = gaitG->startP[i].x;
      (feetPos + i)->y = gaitG->startP[i].y;
      (feetPos + i)->z = gaitG->startP[i].z;

      // (feetVel + i)->x = 0;
      // (feetVel + i)->y = 0;
      // (feetVel + i)->z= 0;
    } else{
      // gaitG->endP[i].x = 
      // gaitG->endP[i].y =
      // gaitG->endP[i].z = 
      // _endXYP.col(i) = _feetCal->calFootPos(i, _vxyGoal, _dYawGoal, (*_phase)(i));

//      float *footPos = gaitGetFootPos(gaitG, i);
//      (feetPos + i)->x = *(footPos + 0);
//      (feetPos + i)->y = *(footPos + 1);
//      (feetPos + i)->z = *(footPos + 2);
//      free(footPos);
      // (feetVel + i)->x = getFootVel;
      // (feetVel + i)->y = getFootVel;
      // (feetVel + i)->z= getFootVel;
    }
    gaitG->pastP[i].x = (feetPos + i)->x;
    gaitG->pastP[i].y = (feetPos + i)->y;
    gaitG->pastP[i].z = (feetPos + i)->z;
    gaitG->phasePast[i] = gaitG->phase[i];
	}
}

void gaitGetFootPos(GaitGenerator* gaitG, uint8_t legID,Point3D *footPos) {
	// float footPos[3] = {0};
//  float *footPos; 
//	footPos = (float *)malloc(sizeof(float) * 3);

  footPos->x = gaitCycloidXYPosition(gaitG->startP[legID].x, gaitG->endP[legID].x, gaitG->phase[legID]);
  footPos->y = gaitCycloidXYPosition(gaitG->startP[legID].y, gaitG->endP[legID].y, gaitG->phase[legID]);
  footPos->z = gaitCycloidZPosition(gaitG->startP[legID].z, gaitG->gaitHeight, gaitG->phase[legID]);

}


// float* gaitGetFootVel(GaitGenerator* gaitG, uint8_t legID) {


// }


float gaitCycloidXYPosition(float startXY, float endXY, float phase) {
	float phasePI = 2 * G_PI * phase;
	return (endXY - startXY) * (phasePI - sin(phasePI)) / (2 * G_PI) + startXY;
} 

//float gaitCycloidXYVelocity(float startXY, float endXY, float phase){
//	float phasePI = 2 * G_PI * phase;
//	return (endXY - startXY) * (1 - cos(phasePI)) / _waveG->getTswing();
//}

float gaitCycloidZPosition(float startZ, float height, float phase) {
	float phasePI = 2 * G_PI * phase;
	return (startZ - (height * (1 - cos(phasePI)) / 2)) ;
}

//float gaitCycloidZVelocity(float height, float phase) {
//	float phasePI = 2 * G_PI * phase;
//	return height * G_PI * sin(phasePI) / _waveG->getTswing();
//}


